#include <local_planner.h>

LocalPlanner::LocalPlanner(ros::NodeHandle& n)
{
    p_parameter = 0.8;
    a_max = 3.0;                //最大加速度3m/s^2
    v_max = 2.0;                //最大速度2m/s
    planner_rate = 20;    //规划频率20Hz
    arrive_error = 0.3;    //距离目标还有0.3m以内认为到达目的地
    aim_distance = 1;    //瞄准1m外的目标点

    listener_ptr.reset(new tf::TransformListener());

    path_sub = n.subscribe("/global_planner_node/path",2,&LocalPlanner::path_callback,this);
    odom_sub = n.subscribe("/odom",100,&LocalPlanner::odom_callback,this);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",1000);
    planner_status = WAIT;

    ros::Rate rate(planner_rate);    //20Hz
    while(ros::ok())
    {
        plan();
        rate.sleep();
        ros::spinOnce();
    }
}

double LocalPlanner::cal_distance(geometry_msgs::PoseStamped& p1,geometry_msgs::PoseStamped& p2)
{
    double distance_square = pow((p1.pose.position.x-p2.pose.position.x),2)+pow((p1.pose.position.y-p2.pose.position.y),2);
    double distance = sqrt(distance_square);
    return distance;
}

void LocalPlanner::find_aim_point(geometry_msgs::PointStamped& aim_point)
{
    for(auto& pose:global_path)
    {
        if(cal_distance(current_pose,pose)>aim_distance)
        {
            aim_point.point = pose.pose.position;
            aim_point.header = pose.header;
            return ;
        }
    }

    aim_point.header = global_path.back().header;
    aim_point.point = global_path.back().pose.position;
    return ;
}

void LocalPlanner::pub_speed(double spd_x,double spd_y)
{
    geometry_msgs::Twist current_vel;
    current_vel.linear.x = spd_x;
    current_vel.linear.y = spd_y;
    current_vel.angular.z = 0;

    vel_pub.publish(current_vel);
}

double LocalPlanner::cal_velocity(double distance_to_goal)    //计算速度绝对值大小
{
    double speed_square = fdb_vel.linear.x*fdb_vel.linear.x+fdb_vel.linear.y*fdb_vel.linear.y;
    if(distance_to_goal<speed_square/(2*a_max))
    {
        double speed = sqrt(speed_square)-a_max/planner_rate;
        return speed;
    }
    double speed = sqrt(speed_square)+a_max/planner_rate;
    speed = std::min(speed,v_max);
    return speed;
}

void LocalPlanner::cal_velocity(geometry_msgs::PointStamped& aim_point,double distance_to_goal)
{
    double speed = cal_velocity(distance_to_goal);
    double ref_x = (1-p_parameter)*fdb_vel.linear.x+p_parameter*aim_point.point.x;
    double ref_y = (1-p_parameter)*fdb_vel.linear.y+p_parameter*aim_point.point.y;
    
    double ref_speed = sqrt(ref_x*ref_x+ref_y*ref_y);
    ref_x = ref_x*speed/ref_speed;
    ref_y = ref_y*speed/ref_speed;
    pub_speed(ref_x,ref_y);
}



void LocalPlanner::plan()
{
    if(planner_status != PLANNING)
        return ;

    double distance_to_goal = cal_distance(current_pose,global_path.back());
    if(distance_to_goal<arrive_error)
    {
        planner_status = WAIT;
        return ;
    }

    geometry_msgs::PointStamped aim_point;
    find_aim_point(aim_point);
    listener_ptr->waitForTransform("base_link","map",ros::Time(0),ros::Duration(3));
    geometry_msgs::PointStamped aim_point_baselink;    //基坐标系下的目标点位置
    listener_ptr->transformPoint("base_link",aim_point,aim_point_baselink);

    cal_velocity(aim_point_baselink,distance_to_goal);


}

void LocalPlanner::odom_callback(const nav_msgs::OdometryConstPtr& msg)
{
    current_pose.pose = msg->pose.pose;
    current_pose.header = msg->header;

    fdb_vel = msg->twist.twist;
}

void LocalPlanner::path_callback(const nav_msgs::PathConstPtr& msg)
{
    global_path.clear();
    if(planner_status != STOP)
    {
        planner_status = PLANNING;
        for(int i=0;i<msg->poses.size();i++){
            global_path.push_back(msg->poses[i]);
        }
        ROS_INFO("receive : %d",global_path.size());
    }
}

